package gm.server;

import gm.server.geom.Circle;
import gm.server.geom.CircleCircleIntersection;
import gm.server.geom.Vector2;

/**
 * Fej.
 *
 * @author imruf84
 */
public class Head {

    /**
     * Első kar.
     */
    private final Arm arm1;
    /**
     * Második kar.
     */
    private final Arm arm2;

    /**
     * Konstruktor.
     *
     * @param arm1 első kar
     * @param arm2 második kar
     */
    public Head(final Arm arm1, final Arm arm2) {
        this.arm1 = arm1;
        this.arm2 = arm2;
    }

    /**
     * Első kar lekérdezése.
     *
     * @return első kar
     */
    public Arm getArm1() {
        return arm1;
    }

    /**
     * Második kar lekérdezése.
     *
     * @return második kar
     */
    public Arm getArm2() {
        return arm2;
    }

    /**
     * Fej helyének a meghatározása a motorokhoz képest.
     *
     * @return fej helyének koordinátái
     * @throws gm.server.WrongPositionException kivétel
     */
    public Vector2 calculatePositionLocal() throws WrongPositionException {

        Arm lArm1 = getArm1();
        Arm lArm2 = getArm2();

        Nut n1 = lArm1.getNut();
        Nut n2 = lArm2.getNut();

        Vector2 nutPos1 = new Vector2(n1.getRod().getPosition(), n1.calculatePositionAbsolute());
        Vector2 nutPos2 = new Vector2(n2.getRod().getPosition(), n2.calculatePositionAbsolute());

        double r1 = lArm1.getLength();
        double r2 = lArm2.getLength();

        CircleCircleIntersection cci = new CircleCircleIntersection(new Circle(nutPos1, r1), new Circle(nutPos2, r2));
        Vector2 p[] = cci.getIntersectionPoints();

        if (2 > p.length) {
            throw new WrongPositionException(
                    getArm1().getNut().getRod().getMotor().getCurrentStepsAbsolute(),
                    getArm2().getNut().getRod().getMotor().getCurrentStepsAbsolute()
            );
        }

        if (1 == p.length) {
            return p[0];
        }

        int index = (!Vector2.isCW(nutPos1, nutPos2, p[0]) ? 0 : 1);

        return p[index];
    }

    /**
     * Fej helyének meghatározása az origóhoz képest.
     *
     * @return fej helyének koordinátái
     * @throws gm.server.WrongPositionException kivétel
     */
    public Vector2 calculatePositionGlobal() throws WrongPositionException {

        Vector2 l = calculatePositionLocal();

        return new Vector2(l.x, getArm1().getNut().getRod().getLength() - l.y);
    }

}
